#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <ros/ros.h>
#include <opencv2/opencv.hpp>
#include <thread>
#include <custom_msg/CMD.h>
#include <custom_msg/GPS.h>
#include "commons/INIReader.h"
#include "commons/cmdline.h"
#include "commons/libar8020.h"
#include "commons/rabbit_consts.h"
#include "commons/timer.h"
#include "commons/x264_encoder.h"

int focus_val = 1;
x264Encoder endocer(BIG_VIDEO_HIGHT, BIG_VIDEO_WIDTH, 3, 15);

void imageCallback(const sensor_msgs::ImageConstPtr& msg) {
  try {
    cv::Mat frame = cv_bridge::toCvShare(msg, "bgr8")->image;
    cv::resize(frame, frame, cv::Size(BIG_VIDEO_HIGHT, BIG_VIDEO_WIDTH));
    int size = endocer.EncodeOneFrame(frame);
    uchar* data = nullptr;
    int res = -1;
    data = endocer.GetEncodedFrame();
    try {
      res = Video_Port_Send_So((char*)data, size);
      std::cout<< "Send h264 with res " << res<< "\r\n"; 
      //LOG(INFO) << "Send h264 with res " << res;
      } catch (...) {
       // LOG(INFO) << "Error send h264";
      }
    //cv::imshow("view", cv_bridge::toCvShare(msg, "bgr8")->image);
    // cv::waitKey(10);
  } catch (cv_bridge::Exception& e) {
    ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
  }
}

void focusCallback(const custom_msg::CMDConstPtr& msg)
{
    int cmd_type = msg->cmd_type;
    int val_type = msg->val_type;
    if(cmd_type==MSG_CMD_TYPE_SUMBOY){
      switch(val_type){
        case MSG_CMD_TYPE_SUMBOY_HW_FOCUS1:
        focus_val = 1;
        break;      
        focus_val = 2;
        break;  
        focus_val = 3;
        break;  
        case MSG_CMD_TYPE_SUMBOY_HW_FOCUS4:
        focus_val = 4;
        break;  
        case MSG_CMD_TYPE_SUMBOY_HW_FOCUS5:
        focus_val = 5;
        break;    
        case MSG_CMD_TYPE_SUMBOY_HW_FOCUS6:
        focus_val = 6;
        break;                                   //hw focus 
        case MSG_CMD_TYPE_SUMBOY_RGB_FOCUS1:
        focus_val = 1;
        break;    
        case MSG_CMD_TYPE_SUMBOY_RGB_FOCUS2:
        focus_val = 2;
        break; 
        case MSG_CMD_TYPE_SUMBOY_RGB_FOCUS3:
        focus_val = 3;
        break; 
        case MSG_CMD_TYPE_SUMBOY_RGB_FOCUS4:
        focus_val = 4;
        break;  
        case MSG_CMD_TYPE_SUMBOY_RGB_FOCUS5:
        focus_val = 5;
        break; 
        case MSG_CMD_TYPE_SUMBOY_RGB_FOCUS6:
        focus_val = 6;
        break;                                //rgb focus                  
      }
    }
}
void gpsCallback(const custom_msg::GPSConstPtr& msg)
{
   
    //std::cout << std::setiosflags(std::ios::fixed) << std::setprecision(7) << "纬度：" << msg->lat <<"N or S:"<<msg->N_S<< " 经度：" << msg->lon<<"E or W:"<<msg->E_W << " groundSpeed：" << msg->groundSpeed << " speedDir：" << msg->speedDir << " gpsAngle：" << msg->gpsAngle << " gpsAngleDir：" << msg->gpsAngleDir << "\n";
}
int main(int argc, char** argv) {
  ros::init(argc, argv, "ai_video_send");
  ros::NodeHandle nh;
  while (Usb_Init_So() < 0) {        //图传模块初始化
    LOG(INFO) << "libar8020 usb init error, reinit in 5 secs";
    std::this_thread::sleep_for(std::chrono::seconds(5));
  }
  image_transport::ImageTransport it(nh);
  image_transport::Subscriber sub =it.subscribe(TOPIC_CAMERA_AI, 1, imageCallback);

  ros::Subscriber sub_cmd = nh.subscribe(TOPIC_CMD, 1000, focusCallback);
  ros::Subscriber sub_gps = nh.subscribe(TOPIC_GPS, 1000, gpsCallback);
  ros::Rate loop_rate(10);
  while (ros::ok())
  {
    Cmd_Port_Send_Txt_FOCUS(focus_val);
    ros::spinOnce();
    loop_rate.sleep();
  }
}
